float v1Org[][];//original vertices
float v1Warped[][];//warped vertices
float v2Org[][];
float v2Warped[][];
float p;//as percentage of warping
int nv,nt;//how many vertices and triangles are in action
float triangles[][];//all triangles will have 3 index from the original and warped arrays

void setup () {
  size(400,400);
  int i=0,j=0;
  nv=5;
  nt=4;
  v1Org = new float[nv][2];
  v1Warped = new float[nv][2];
  v2Org = new float[nv][2];
  v2Warped = new float[nv][2];
  triangles = new float[nt][3];
  p=0;
  assignCorners();
}

void draw() {
  background(0);
}

void findColorPoint(float frp[],float A1[],float B1[],float C1[],float top[],float A2[],float B2[],float C2[]) {
  //1.find the needed vectors,lengths,degrees of the triangle
  float AB1[] = new float[] {B1[0]-A1[0],B1[1]-A1[1],0};
  float AB1L = DogaMakeUnitVector(AB1);
  float AC1[] = new float[] {C1[0]-A1[0],C1[1]-A1[1],0};
  float AC1L = DogaMakeUnitVector(AC1);
  float AP[] = new float[] {frp[0]-A1[0],frp[1]-A1[1],0};
  float APL = DogaMakeUnitVector(AP);
  float cosAlfa = DogaDotProduct(AB1,AP);
  float sinAlfa = sqrt(1-cosAlfa*cosAlfa);
  float cosBeta = DogaDotProduct(AP,AC1);
  float sinBeta = sqrt(1-cosBeta*cosBeta);
  //2.find the lengths needed for finding coefficients
  float d1 = APL*sinBeta;
  float AB1mid = d1/(sinAlfa*cosBeta+cosAlfa*sinBeta);//since sin(alfa+beta)=sin(alfa)*cos(beta)+cos(alfa)*sin(beta)
  float AC1mid = APL*cosBeta - sqrt(AB1mid*AB1mid-d1*d1);
  //3.find the coefficients
  float ABcoef=(AB1mid/AB1L);
  float ACcoef=(AC1mid/AC1L);
  //4.find the new point according to new triangles coordinates
  float AB2[] = new float[] {B2[0]-A2[0],B2[1]-A2[1],0};
  float AB2L = DogaMakeUnitVector(AB2);  
  float AC2[] = new float[] {C2[0]-A2[0],C2[1]-A2[1],0};
  float AC2L = DogaMakeUnitVector(AC2);
  top[0] = A2[0] + ABcoef*AB2[0]*AB2L + ACcoef*AC2[0]*AC2L;
  top[1] = A2[1] + ABcoef*AB2[1]*AB2L + ACcoef*AC2[1]*AC2L;
}

void assignCorners() {
  v1Org[0] = new float[]{0,0};  //0 will be top-left corner
  v1Org[1] = new float[]{width,0};  //1 will be top-right corner
  v1Org[2] = new float[]{width,height};  //2 will be bottom-right corner
  v1Org[3] = new float[]{0,height};  //3 will be bottom-left corner
  v1Warped[0] = new float[]{0,0};  //0 will be top-left corner
  v1Warped[1] = new float[]{width,0};  //1 will be top-right corner
  v1Warped[2] = new float[]{width,height};  //2 will be bottom-right corner
  v1Warped[3] = new float[]{0,height};  //3 will be bottom-left corner
  v2Org[0] = new float[]{0,0};  //0 will be top-left corner
  v2Org[1] = new float[]{width,0};  //1 will be top-right corner
  v2Org[2] = new float[]{width,height};  //2 will be bottom-right corner
  v2Org[3] = new float[]{0,height};  //3 will be bottom-left corner
  v2Warped[0] = new float[]{0,0};  //0 will be top-left corner
  v2Warped[1] = new float[]{width,0};  //1 will be top-right corner
  v2Warped[2] = new float[]{width,height};  //2 will be bottom-right corner
  v2Warped[3] = new float[]{0,height};  //3 will be bottom-left corner
}

float DogaDotProduct(float V1[],float V2[]) {return V1[0]*V2[0]+V1[1]*V2[1]+V1[2]*V2[2];}
float DogaMakeUnitVector(float V[]) {
  float w=0;
  w = sqrt(DogaDotProduct(V,V));
  if (w!=0) {
    V[0] = V[0]/w;
    V[1] = V[1]/w;
    V[2] = V[2]/w;
  }
  else {
    V[0] = 0;
    V[1] = 0;
    V[2] = 0;
  }
  return w;
}
